package org.electrode.system;

import org.electrode.base.ArcadeDriveControl;
import org.electrode.base.DriveControl;
import org.electrode.base.MotorChange;
import org.electrode.base.PIDValues;
import org.electrode.base.XboxJoystick;
import org.electrode.common.RobotSystem;

import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Encoder.PIDSourceParameter;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.tables.ITable;

public class Drive extends RobotSystem {
	private static final int OFF = 0;
	private static final double TEST_ON = 0.25;
	private static final int ONE_MINUTE = 60000;
	private static final int INCREMENT = 250;

	private final DriveControl control;
	private Victor leftMotor;
	private Victor rightMotor;
	private Encoder leftEncoder;
	private Encoder rightEncoder;
	private PIDValues pid;
	private PIDController leftPid;
	private PIDController rightPid;
	// autonomousPeriodic variables
	// accel in in/mSec^2. 1G is 0.000384
	private double aMax = 0.000060; // max acceleration (in/mSec^2)
	private double vMax = 0.060; // max velocity (in/mSec)
	private double tMax; // time to reach max velocity
	private double sMax; // distance to reach max velocity
	private double distance; // to destination (inches)
	private double tStart; // time at start of autonomous
	private double sdStart; // distance to start decelerating
	private double tdStart; // time to start decelerating
	private double tTotal; // total time to destination

	public Drive(XboxJoystick xbox) {
		control = new ArcadeDriveControl(xbox);
	}

	public void init() {
		leftMotor = new Victor(1);
		rightMotor = new Victor(2);
		leftEncoder = new Encoder(1, 2, true, EncodingType.k4X);
		rightEncoder = new Encoder(3, 4, false, EncodingType.k4X);

		ITable driveTable = NetworkTable.getTable("Drive");

		leftMotor.initTable(driveTable.getSubTable("LeftMotor"));
		rightMotor.initTable(driveTable.getSubTable("RightMotor"));

		leftEncoder.setDistancePerPulse(0.0808);
		leftEncoder.setPIDSourceParameter(PIDSourceParameter.kDistance);
		leftEncoder.initTable(driveTable.getSubTable("LeftEncoder"));

		rightEncoder.setDistancePerPulse(0.0788);
		rightEncoder.setPIDSourceParameter(PIDSourceParameter.kDistance);
		rightEncoder.initTable(driveTable.getSubTable("RightEncoder"));

		pid = new PIDValues(0.05, 0.001, 0.125);
		leftPid = new PIDController(pid.getP(), pid.getI(), pid.getD(),
				leftEncoder, leftMotor);
		leftPid.initTable(NetworkTable.getTable("LeftPID"));

		rightPid = new PIDController(-pid.getP(), -pid.getI(), -pid.getD(),
				rightEncoder, rightMotor);
		rightPid.initTable(NetworkTable.getTable("RightPID"));

		SmartDashboard.putNumber("AUTO_DISTANCE", 50);
		SmartDashboard.putNumber("aMax", 60); // in/sec^2
		SmartDashboard.putNumber("vMax", 60); // in/sec

	}

	public void enableTeleop() {
		leftEncoder.reset();
		rightEncoder.reset();
		leftEncoder.start();
		rightEncoder.start();
	}

	public void periodicTeleop() {
		leftMotor.updateTable();
		rightMotor.updateTable();
		leftEncoder.updateTable();
		rightEncoder.updateTable();
		MotorChange change = control.updateMotors();
		leftMotor.set(change.getLeft());
		rightMotor.set(change.getRight());
		control.updateTable();
	}

	public void enableAuto() {
		/*
		 * pid.set(leftPid); pid.setReversed(rightPid);
		 * 
		 * startEncoders();
		 * 
		 * leftPid.enable(); rightPid.enable();
		 * 
		 * leftPid.setSetpoint(0); rightPid.setSetpoint(0);
		 */
		/*
		 * distance = SmartDashboard.getNumber("AUTO_DISTANCE"); aMax =
		 * SmartDashboard.getNumber("aMax") / 1e6; vMax =
		 * SmartDashboard.getNumber("vMax") / 1e3; tMax = vMax / aMax; sMax =
		 * aMax * tMax * tMax / 2.0; sdStart = distance - sMax; tStart =
		 * System.currentTimeMillis(); tdStart = tStart + tMax + (distance - 2 *
		 * sMax) / vMax; tTotal = tdStart + tMax;
		 */
	}

	public void disable() {
		leftPid.disable();
		rightPid.disable();
		leftEncoder.stop();
		rightEncoder.stop();
	}

	public void periodicAuto() {
		/*
		 * leftPid.updateTable(); rightPid.updateTable();
		 * leftEncoder.updateTable(); rightEncoder.updateTable();
		 */
		/*
		 * double currentT = System.currentTimeMillis() - tStart; double
		 * setPoint;
		 * 
		 * if (currentT <= tMax) { setPoint = aMax * currentT * currentT / 2.0;
		 * } else if (currentT < tdStart) { setPoint = vMax * (currentT - tMax)
		 * + sMax; } else if (currentT <= tTotal) { double t = currentT -
		 * tdStart; setPoint = aMax * t * t / 2.0 + vMax * t + sdStart; } else {
		 * setPoint = distance; } leftPid.setSetpoint(setPoint);
		 * rightPid.setSetpoint(setPoint);
		 */
	}

	public String getName() {
		return "Drive";
	}

	public void enableTest() {
		startEncoders();
		testMotor(leftMotor);
		testMotor(rightMotor);
	}

	private void startEncoders() {
		leftEncoder.reset();
		rightEncoder.reset();
		leftEncoder.start();
		rightEncoder.start();
	}

	private void testMotor(Victor motor) {
		motor.set(TEST_ON);
		long then = System.currentTimeMillis();
		try {
			while ((System.currentTimeMillis() - then) <= ONE_MINUTE) {
				Thread.sleep(INCREMENT);
				motor.updateTable();
				leftEncoder.updateTable();
				rightEncoder.updateTable();
			}
		} catch (InterruptedException ex) {
			// do nothing
		}
		motor.set(OFF);
		control.updateTable();
	}
}
